<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package imagingbook.calibration.zhang;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import java.io.StringWriter;<a name="line.3"></a>
<span class="sourceLineNo">004</span>import java.util.Arrays;<a name="line.4"></a>
<span class="sourceLineNo">005</span><a name="line.5"></a>
<span class="sourceLineNo">006</span>import org.apache.commons.math3.geometry.euclidean.threed.Rotation;<a name="line.6"></a>
<span class="sourceLineNo">007</span>import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;<a name="line.7"></a>
<span class="sourceLineNo">008</span>import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;<a name="line.8"></a>
<span class="sourceLineNo">009</span>import org.apache.commons.math3.linear.MatrixUtils;<a name="line.9"></a>
<span class="sourceLineNo">010</span>import org.apache.commons.math3.linear.RealMatrix;<a name="line.10"></a>
<span class="sourceLineNo">011</span>import org.apache.commons.math3.linear.RealVector;<a name="line.11"></a>
<span class="sourceLineNo">012</span><a name="line.12"></a>
<span class="sourceLineNo">013</span>import imagingbook.lib.math.Matrix;<a name="line.13"></a>
<span class="sourceLineNo">014</span><a name="line.14"></a>
<span class="sourceLineNo">015</span>/**<a name="line.15"></a>
<span class="sourceLineNo">016</span> * Instances of this class represent extrinsic camera (view) parameters.<a name="line.16"></a>
<span class="sourceLineNo">017</span> * @author WB<a name="line.17"></a>
<span class="sourceLineNo">018</span> *<a name="line.18"></a>
<span class="sourceLineNo">019</span> */<a name="line.19"></a>
<span class="sourceLineNo">020</span>public class ViewTransform {<a name="line.20"></a>
<span class="sourceLineNo">021</span>        <a name="line.21"></a>
<span class="sourceLineNo">022</span>        private static final double OrthogonalityThreshold = 0.01;<a name="line.22"></a>
<span class="sourceLineNo">023</span>        <a name="line.23"></a>
<span class="sourceLineNo">024</span>        private final Rotation rotation;<a name="line.24"></a>
<span class="sourceLineNo">025</span>        private final double[] translation;<a name="line.25"></a>
<span class="sourceLineNo">026</span>        <a name="line.26"></a>
<span class="sourceLineNo">027</span>        // ----------------------------------------------------------------------------------<a name="line.27"></a>
<span class="sourceLineNo">028</span>        <a name="line.28"></a>
<span class="sourceLineNo">029</span>        public ViewTransform() {<a name="line.29"></a>
<span class="sourceLineNo">030</span>                this.rotation = Rotation.IDENTITY;<a name="line.30"></a>
<span class="sourceLineNo">031</span>                this.translation = new double[3];<a name="line.31"></a>
<span class="sourceLineNo">032</span>        }<a name="line.32"></a>
<span class="sourceLineNo">033</span>        <a name="line.33"></a>
<span class="sourceLineNo">034</span>        public ViewTransform(double rX, double rY, double rZ, double tX, double tY, double tZ) {<a name="line.34"></a>
<span class="sourceLineNo">035</span>                this.rotation = makeRotation(new double[] {rX, rY, rZ});<a name="line.35"></a>
<span class="sourceLineNo">036</span>                this.translation = new double[] {tX, tY, tZ};<a name="line.36"></a>
<span class="sourceLineNo">037</span>        }<a name="line.37"></a>
<span class="sourceLineNo">038</span>        <a name="line.38"></a>
<span class="sourceLineNo">039</span>        public ViewTransform(Rotation rot, double[] t) {<a name="line.39"></a>
<span class="sourceLineNo">040</span>                this.rotation = rot;<a name="line.40"></a>
<span class="sourceLineNo">041</span>                translation = t;<a name="line.41"></a>
<span class="sourceLineNo">042</span>        }<a name="line.42"></a>
<span class="sourceLineNo">043</span>        <a name="line.43"></a>
<span class="sourceLineNo">044</span>        public ViewTransform(RealMatrix RT) {   // RT is of size 3 x 4 (a homography)<a name="line.44"></a>
<span class="sourceLineNo">045</span>                if (RT.getRowDimension() != 3 || RT.getColumnDimension() != 4) {<a name="line.45"></a>
<span class="sourceLineNo">046</span>                        throw new IllegalArgumentException("View transform matrix must be 3 x 4");<a name="line.46"></a>
<span class="sourceLineNo">047</span>                }<a name="line.47"></a>
<span class="sourceLineNo">048</span>                RealMatrix R = RT.getSubMatrix(0, 2, 0, 2);<a name="line.48"></a>
<span class="sourceLineNo">049</span>                rotation = new Rotation(R.getData(), OrthogonalityThreshold);<a name="line.49"></a>
<span class="sourceLineNo">050</span>                translation = RT.getColumnVector(3).toArray();<a name="line.50"></a>
<span class="sourceLineNo">051</span>        }<a name="line.51"></a>
<span class="sourceLineNo">052</span>        <a name="line.52"></a>
<span class="sourceLineNo">053</span>        public ViewTransform(RealMatrix R, RealVector t) {      // R is of size 3 x 3 , t of size 3 x 1<a name="line.53"></a>
<span class="sourceLineNo">054</span>                this(new Rotation(R.getData(), OrthogonalityThreshold), t.toArray());<a name="line.54"></a>
<span class="sourceLineNo">055</span>//              if (R.getRowDimension() != 3 || R.getColumnDimension() != 3) {<a name="line.55"></a>
<span class="sourceLineNo">056</span>//                      throw new IllegalArgumentException("Rotation matrix must be 3 x 3");<a name="line.56"></a>
<span class="sourceLineNo">057</span>//              }<a name="line.57"></a>
<span class="sourceLineNo">058</span>//              if (t.getDimension() != 3) {<a name="line.58"></a>
<span class="sourceLineNo">059</span>//                      throw new IllegalArgumentException("Translation vector must be of dimension 3");<a name="line.59"></a>
<span class="sourceLineNo">060</span>//              }<a name="line.60"></a>
<span class="sourceLineNo">061</span>//              rotation = new Rotation(R.getData(), OrthogonalityThreshold);<a name="line.61"></a>
<span class="sourceLineNo">062</span>//              translation = t.toArray();<a name="line.62"></a>
<span class="sourceLineNo">063</span>        }<a name="line.63"></a>
<span class="sourceLineNo">064</span>        <a name="line.64"></a>
<span class="sourceLineNo">065</span>        public ViewTransform(double[] w) {<a name="line.65"></a>
<span class="sourceLineNo">066</span>                this.rotation = makeRotation(w);<a name="line.66"></a>
<span class="sourceLineNo">067</span>                this.translation = Arrays.copyOfRange(w, 3, 6);<a name="line.67"></a>
<span class="sourceLineNo">068</span>        }<a name="line.68"></a>
<span class="sourceLineNo">069</span>        <a name="line.69"></a>
<span class="sourceLineNo">070</span>        // ----------------------------------------------------------------------------------<a name="line.70"></a>
<span class="sourceLineNo">071</span>        <a name="line.71"></a>
<span class="sourceLineNo">072</span>        private Rotation makeRotation(double[] w) {<a name="line.72"></a>
<span class="sourceLineNo">073</span>                Vector3D axis = new Vector3D(w[0], w[1], w[2]);<a name="line.73"></a>
<span class="sourceLineNo">074</span>                double angle = axis.getNorm();<a name="line.74"></a>
<span class="sourceLineNo">075</span>                //return new Rotation(axis, angle);<a name="line.75"></a>
<span class="sourceLineNo">076</span>                return new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR);<a name="line.76"></a>
<span class="sourceLineNo">077</span>                 <a name="line.77"></a>
<span class="sourceLineNo">078</span>        }<a name="line.78"></a>
<span class="sourceLineNo">079</span>                <a name="line.79"></a>
<span class="sourceLineNo">080</span>        protected int getParameterLength() {<a name="line.80"></a>
<span class="sourceLineNo">081</span>                return 6;<a name="line.81"></a>
<span class="sourceLineNo">082</span>        }<a name="line.82"></a>
<span class="sourceLineNo">083</span>        <a name="line.83"></a>
<span class="sourceLineNo">084</span>        protected double[] getParameters() {<a name="line.84"></a>
<span class="sourceLineNo">085</span>                //double[] rotAxis = rotation.getAxis().toArray();<a name="line.85"></a>
<span class="sourceLineNo">086</span>                double[] rotAxis = rotation.getAxis(RotationConvention.VECTOR_OPERATOR).toArray();<a name="line.86"></a>
<span class="sourceLineNo">087</span>                double rotAngle = rotation.getAngle();<a name="line.87"></a>
<span class="sourceLineNo">088</span>                return new double[] {<a name="line.88"></a>
<span class="sourceLineNo">089</span>                        rotAxis[0] * rotAngle,<a name="line.89"></a>
<span class="sourceLineNo">090</span>                        rotAxis[1] * rotAngle,<a name="line.90"></a>
<span class="sourceLineNo">091</span>                        rotAxis[2] * rotAngle,<a name="line.91"></a>
<span class="sourceLineNo">092</span>                        translation[0], translation[1], translation[2]};<a name="line.92"></a>
<span class="sourceLineNo">093</span>        }<a name="line.93"></a>
<span class="sourceLineNo">094</span>        <a name="line.94"></a>
<span class="sourceLineNo">095</span>        public Rotation getRotation() {<a name="line.95"></a>
<span class="sourceLineNo">096</span>                return rotation;<a name="line.96"></a>
<span class="sourceLineNo">097</span>        }<a name="line.97"></a>
<span class="sourceLineNo">098</span>        <a name="line.98"></a>
<span class="sourceLineNo">099</span>        public double[] getRotationAxis() {<a name="line.99"></a>
<span class="sourceLineNo">100</span>                //double[] rotAxis = rotation.getAxis().toArray();<a name="line.100"></a>
<span class="sourceLineNo">101</span>                double[] rotAxis = rotation.getAxis(RotationConvention.VECTOR_OPERATOR).toArray();<a name="line.101"></a>
<span class="sourceLineNo">102</span>                double rotAngle = rotation.getAngle();<a name="line.102"></a>
<span class="sourceLineNo">103</span>                rotAxis[0] *= rotAngle;<a name="line.103"></a>
<span class="sourceLineNo">104</span>                rotAxis[1] *= rotAngle;<a name="line.104"></a>
<span class="sourceLineNo">105</span>                rotAxis[2] *= rotAngle;<a name="line.105"></a>
<span class="sourceLineNo">106</span>                return rotAxis;<a name="line.106"></a>
<span class="sourceLineNo">107</span>        }<a name="line.107"></a>
<span class="sourceLineNo">108</span>        <a name="line.108"></a>
<span class="sourceLineNo">109</span>        public RealMatrix getRotationMatrix() {<a name="line.109"></a>
<span class="sourceLineNo">110</span>                double[][] R = rotation.getMatrix();<a name="line.110"></a>
<span class="sourceLineNo">111</span>                return MatrixUtils.createRealMatrix(R);<a name="line.111"></a>
<span class="sourceLineNo">112</span>        }<a name="line.112"></a>
<span class="sourceLineNo">113</span>        <a name="line.113"></a>
<span class="sourceLineNo">114</span>        public double[] getTranslation() {<a name="line.114"></a>
<span class="sourceLineNo">115</span>                return translation;<a name="line.115"></a>
<span class="sourceLineNo">116</span>        }<a name="line.116"></a>
<span class="sourceLineNo">117</span>        <a name="line.117"></a>
<span class="sourceLineNo">118</span>        public RealVector getTranslationVector() {<a name="line.118"></a>
<span class="sourceLineNo">119</span>                return MatrixUtils.createRealVector(translation);<a name="line.119"></a>
<span class="sourceLineNo">120</span>        }<a name="line.120"></a>
<span class="sourceLineNo">121</span>        <a name="line.121"></a>
<span class="sourceLineNo">122</span>        // ----------------------------------------------------------------------------------<a name="line.122"></a>
<span class="sourceLineNo">123</span>        <a name="line.123"></a>
<span class="sourceLineNo">124</span>        /**<a name="line.124"></a>
<span class="sourceLineNo">125</span>         * Moves point XYZ from 3D world coordinates to 3D camera coordinates,<a name="line.125"></a>
<span class="sourceLineNo">126</span>         * as specified by the transformations of this view.<a name="line.126"></a>
<span class="sourceLineNo">127</span>         * No projection is involved.<a name="line.127"></a>
<span class="sourceLineNo">128</span>         * @param XYZ a 3D (world) point<a name="line.128"></a>
<span class="sourceLineNo">129</span>         * @return the given world point mapped to 3D camera coordinates<a name="line.129"></a>
<span class="sourceLineNo">130</span>         */<a name="line.130"></a>
<span class="sourceLineNo">131</span>        protected double[] applyTo(double[] XYZ) {      // 3D vector XYZ assumed<a name="line.131"></a>
<span class="sourceLineNo">132</span>                double[] XYZc = new double[3];<a name="line.132"></a>
<span class="sourceLineNo">133</span>                rotation.applyTo(XYZ, XYZc);<a name="line.133"></a>
<span class="sourceLineNo">134</span>                for (int i = 0; i &lt; 3; i++) {<a name="line.134"></a>
<span class="sourceLineNo">135</span>                        XYZc[i] = XYZc[i] + translation[i];<a name="line.135"></a>
<span class="sourceLineNo">136</span>                }<a name="line.136"></a>
<span class="sourceLineNo">137</span>                return XYZc;<a name="line.137"></a>
<span class="sourceLineNo">138</span>        }<a name="line.138"></a>
<span class="sourceLineNo">139</span>        <a name="line.139"></a>
<span class="sourceLineNo">140</span>        public String toString() {<a name="line.140"></a>
<span class="sourceLineNo">141</span>                RealMatrix R = this.getRotationMatrix();<a name="line.141"></a>
<span class="sourceLineNo">142</span>                RealVector T = this.getTranslationVector();<a name="line.142"></a>
<span class="sourceLineNo">143</span>                StringWriter writer = new StringWriter();<a name="line.143"></a>
<span class="sourceLineNo">144</span>                writer.append("R = \n");<a name="line.144"></a>
<span class="sourceLineNo">145</span>                writer.append(Matrix.toString(R.getData()));<a name="line.145"></a>
<span class="sourceLineNo">146</span>                writer.append("\n");<a name="line.146"></a>
<span class="sourceLineNo">147</span>                writer.append("T = ");<a name="line.147"></a>
<span class="sourceLineNo">148</span>                writer.append(Matrix.toString(T.toArray()));<a name="line.148"></a>
<span class="sourceLineNo">149</span>                return writer.toString();<a name="line.149"></a>
<span class="sourceLineNo">150</span>        }<a name="line.150"></a>
<span class="sourceLineNo">151</span>        <a name="line.151"></a>
<span class="sourceLineNo">152</span>        // ------------------------------------------------------------------<a name="line.152"></a>
<span class="sourceLineNo">153</span>        <a name="line.153"></a>
<span class="sourceLineNo">154</span>        /**<a name="line.154"></a>
<span class="sourceLineNo">155</span>         * For testing only.<a name="line.155"></a>
<span class="sourceLineNo">156</span>         * @param args ignored<a name="line.156"></a>
<span class="sourceLineNo">157</span>         */<a name="line.157"></a>
<span class="sourceLineNo">158</span>        public static void main(String[] args) {<a name="line.158"></a>
<span class="sourceLineNo">159</span>                double[] w1 = {1.2, -0.5, 0.9, 10, 20, 30};<a name="line.159"></a>
<span class="sourceLineNo">160</span>                System.out.println("w1 = " + Matrix.toString(w1));<a name="line.160"></a>
<span class="sourceLineNo">161</span>                ViewTransform view = new ViewTransform(w1);<a name="line.161"></a>
<span class="sourceLineNo">162</span>                double[] w2 = view.getParameters();<a name="line.162"></a>
<span class="sourceLineNo">163</span>                System.out.println("w2 = " + Matrix.toString(w2));<a name="line.163"></a>
<span class="sourceLineNo">164</span>        }<a name="line.164"></a>
<span class="sourceLineNo">165</span><a name="line.165"></a>
<span class="sourceLineNo">166</span>        <a name="line.166"></a>
<span class="sourceLineNo">167</span>}<a name="line.167"></a>




























































</pre>
</div>
</body>
</html>
